/**
 * @file main.cpp
 * @brief 路径规划库主程序
 * @details 实现地图加载、算法选择、交互式路径规划功能
 */
/**
 * @file main.cpp
 * @brief 路径规划库主程序 - 修复const问题版本
 */
/**
 * @file main.cpp
 * @brief 路径规划库主程序 - 添加详细调试信息
 */

#include "core/Map.h"
#include "core/Visualization.h"
#include "planners/DijkstraPlanner.h"
#include "planners/AStarPlanner.h"
#include "planners/HybridAStarPlanner.h"
#include "planners/ThetaStarPlanner.h"
#include "planners/RRTStarPlanner.h"
#include "planners/JPSPlanner.h"
#include <memory>
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>

using namespace PathPlanning;

// 全局变量用于鼠标交互
std::shared_ptr<Visualizer>  visualizer;
std::shared_ptr<PlannerBase> currentPlanner;
std::vector<Point>           selectedPoints; // 0: start, 1: goal
bool                         planningInProgress = false;
std::string                  currentPlannerName = "AStar";
PlanningResult               lastPlanningResult;      // 保存上一次的规划结果
std::vector<Pose>            currentExplorationNodes; // 当前搜索过程中的探索节点

// 鼠标回调函数
void mouseCallback(int event, int x, int y, int flags, void* userdata)
{
    if (planningInProgress)
    {
        std::cout << "Planning in progress, please wait..." << std::endl;
        return;
    }

    if (event == cv::EVENT_LBUTTONDOWN)
    {
        Point worldPoint = visualizer->imageToWorld(cv::Point(x, y));

        std::cout << "Mouse clicked at image (" << x << ", " << y << ") -> world (" << worldPoint.x << ", "
                  << worldPoint.y << ")" << std::endl;

        // 检查点是否在地图范围内且不是障碍物
        if (!visualizer->getMap()->isInBounds(worldPoint))
        {
            std::cout << "Selected point is out of map bounds" << std::endl;
            return;
        }

        if (visualizer->getMap()->isObstacle(worldPoint))
        {
            std::cout << "Selected point is in obstacle, please choose another location" << std::endl;
            return;
        }

        if (selectedPoints.size() < 2)
        {
            selectedPoints.push_back(worldPoint);
            std::cout << "Selected point " << selectedPoints.size() << ": (" << worldPoint.x << ", " << worldPoint.y
                      << ")" << std::endl;

            // 实时显示选中的点
            PlanningResult tempResult = lastPlanningResult; // 保留之前的规划结果
            if (selectedPoints.size() == 1)
            {
                // 只选择了起点，显示起点
                tempResult.path = {Pose(selectedPoints[0].x, selectedPoints[0].y, 0)};
            }
            else if (selectedPoints.size() == 2)
            {
                // 选择了起点和终点，显示两点
                tempResult.path = {Pose(selectedPoints[0].x, selectedPoints[0].y, 0),
                                   Pose(selectedPoints[1].x, selectedPoints[1].y, 0)};
            }
            visualizer->updateDisplay(tempResult, selectedPoints.size() > 0 ? selectedPoints[0] : Point(),
                                      selectedPoints.size() > 1 ? selectedPoints[1] : Point(), currentPlannerName);
        }

        // 如果已经选择了两个点，开始规划
        if (selectedPoints.size() == 2)
        {
            planningInProgress = true;
            currentExplorationNodes.clear(); // 清空当前探索节点

            std::cout << "\n=== Starting Path Planning ===" << std::endl;
            std::cout << "Start: (" << selectedPoints[0].x << ", " << selectedPoints[0].y << ")" << std::endl;
            std::cout << "Goal: (" << selectedPoints[1].x << ", " << selectedPoints[1].y << ")" << std::endl;
            std::cout << "Planner: " << currentPlannerName << std::endl;
            std::cout << "Current planner pointer: " << currentPlanner.get() << std::endl;

            // 检查规划器是否有效
            if (!currentPlanner)
            {
                std::cerr << "ERROR: Current planner is null!" << std::endl;
                planningInProgress = false;
                return;
            }

            // 设置可视化回调
            currentPlanner->setVisualizationCallback([&](const PlanningResult& partialResult) {
                std::cout << "Visualization callback called, explored nodes: " << partialResult.exploredNodes.size()
                          << ", iterations: " << partialResult.iterations << std::endl;

                // 更新当前探索节点
                if (!partialResult.exploredNodes.empty())
                {
                    // 将新的探索节点添加到当前探索节点列表中
                    currentExplorationNodes.insert(currentExplorationNodes.end(), partialResult.exploredNodes.begin(),
                                                   partialResult.exploredNodes.end());
                }

                // 创建用于显示的临时结果
                PlanningResult displayResult = partialResult;
                displayResult.exploredNodes  = currentExplorationNodes;

                visualizer->updateDisplay(displayResult, selectedPoints[0], selectedPoints[1], currentPlannerName);

                // 短暂延迟以显示搜索过程
                cv::waitKey(1);
            });

            std::cout << "Calling planner->plan()..." << std::endl;

            // 执行路径规划
            PlanningResult result;
            try
            {
                result = currentPlanner->plan(selectedPoints[0], selectedPoints[1]);
            }
            catch (const std::exception& e)
            {
                std::cerr << "Exception during planning: " << e.what() << std::endl;
                planningInProgress = false;
                return;
            }

            std::cout << "Planning completed, success: " << result.success << std::endl;

            // 保存规划结果
            lastPlanningResult               = result;
            lastPlanningResult.exploredNodes = currentExplorationNodes; // 保存所有探索节点

            // 显示最终结果（包含搜索过程和最终路径）
            visualizer->displayResult(lastPlanningResult, selectedPoints[0], selectedPoints[1], currentPlannerName);

            planningInProgress = false;

            // 显示规划统计信息
            std::cout << "\n=== Planning Result ===" << std::endl;
            std::cout << "Success: " << (result.success ? "Yes" : "No") << std::endl;
            if (result.success)
            {
                std::cout << "Path Length: " << result.pathLength << " meters" << std::endl;
                std::cout << "Planning Time: " << result.planningTime << " seconds" << std::endl;
                std::cout << "Iterations: " << result.iterations << std::endl;
                std::cout << "Path Points: " << result.path.size() << std::endl;
                std::cout << "Explored Nodes: " << currentExplorationNodes.size() << std::endl;
            }
            else
            {
                std::cout << "Failed to find path" << std::endl;
                std::cout << "Message: " << result.message << std::endl;
            }
        }
    }

    // 右键清除选择
    if (event == cv::EVENT_RBUTTONDOWN)
    {
        selectedPoints.clear();
        lastPlanningResult = PlanningResult(); // 清除之前的规划结果
        currentExplorationNodes.clear();       // 清除探索节点
        std::cout << "Cleared selected points and planning results" << std::endl;

        // 重新显示空白地图
        PlanningResult emptyResult;
        visualizer->updateDisplay(emptyResult, Point(), Point(), currentPlannerName);
    }
}

// 创建规划器实例
std::shared_ptr<PlannerBase> createPlanner(const std::string& plannerName, std::shared_ptr<Map> map)
{
    std::cout << "Creating planner: " << plannerName << std::endl;

    if (plannerName == "Dijkstra")
    {
        auto planner = std::make_shared<DijkstraPlanner>(map);
        std::cout << "Dijkstra planner created successfully" << std::endl;
        return planner;
    }
    else if (plannerName == "AStar")
    {
        auto planner = std::make_shared<AStarPlanner>(map);
        std::cout << "AStar planner created successfully" << std::endl;
        return planner;
    }
    else if (plannerName == "HybridAStar")
    {
        auto planner = std::make_shared<HybridAStarPlanner>(map);
        std::cout << "HybridAStar planner created successfully" << std::endl;
        return planner;
    }
    else if (plannerName == "ThetaStar")
    {
        auto planner = std::make_shared<ThetaStarPlanner>(map);
        std::cout << "ThetaStar planner created successfully" << std::endl;
        return planner;
    }
    else if (plannerName == "RRTStar")
    {
        auto planner = std::make_shared<RRTStarPlanner>(map);
        std::cout << "RRTStar planner created successfully" << std::endl;
        return planner;
    }
    else if (plannerName == "JPS")
    {
        auto planner = std::make_shared<JPSPlanner>(map);
        std::cout << "JPS planner created successfully" << std::endl;
        return planner;
    }
    else
    {
        std::cerr << "Unknown planner: " << plannerName << ", using AStar as default" << std::endl;
        auto planner = std::make_shared<AStarPlanner>(map);
        std::cout << "Default AStar planner created successfully" << std::endl;
        return planner;
    }
}

int main(int argc, char* argv[])
{
    std::cout << "=== Path Planning Library Demo ===" << std::endl;

    // 默认参数
    std::string mapFile     = "../maps/test_map.jpg";
    std::string plannerName = "AStar";

    // 解析命令行参数
    if (argc > 1)
        mapFile = argv[1];
    if (argc > 2)
        plannerName = argv[2];

    std::cout << "Loading map from: " << mapFile << std::endl;

    // 加载地图
    auto map = std::make_shared<Map>();
    if (!map->loadFromBMP(mapFile))
    {
        std::cerr << "Failed to load map: " << mapFile << std::endl;
        return -1;
    }
    map->printInfo();

    // 创建规划器
    currentPlanner     = createPlanner(plannerName, map);
    currentPlannerName = plannerName;

    // 初始化可视化
    visualizer = std::make_shared<Visualizer>(map);
    if (!visualizer->initialize(800, 600))
    {
        std::cerr << "Failed to initialize visualizer" << std::endl;
        return -1;
    }

    // 设置鼠标回调
    visualizer->setMouseCallback(mouseCallback);

    // 显示使用说明
    std::cout << "\n=== Usage Instructions ===" << std::endl;
    std::cout << "Left click: Select start and goal points" << std::endl;
    std::cout << "Right click: Clear selection" << std::endl;
    std::cout << "Press 'q' or ESC to quit" << std::endl;
    std::cout << "Press '1-6' to switch planners:" << std::endl;
    std::cout << "  1: Dijkstra, 2: A*, 3: Hybrid A*, 4: Theta*, 5: RRT*, 6: JPS" << std::endl;
    std::cout << "Press 'c' to clear current path and points" << std::endl;
    std::cout << "Press 'r' to reset to default view" << std::endl;

    // 主循环
    bool running = true;
    while (running)
    {
        // 显示当前状态
        visualizer->updateDisplay(lastPlanningResult, selectedPoints.size() > 0 ? selectedPoints[0] : Point(),
                                  selectedPoints.size() > 1 ? selectedPoints[1] : Point(), currentPlannerName);

        // 检查键盘输入
        int key = cv::waitKey(30) & 0xFF;

        switch (key)
        {
        case 'q':
        case 27: // ESC
            std::cout << "Exiting..." << std::endl;
            running = false;
            break;

        case 'c':
            selectedPoints.clear();
            lastPlanningResult = PlanningResult();
            currentExplorationNodes.clear();
            std::cout << "Cleared selected points and planning results" << std::endl;
            break;

        case 'r':
            selectedPoints.clear();
            lastPlanningResult = PlanningResult();
            currentExplorationNodes.clear();
            std::cout << "Reset to default view" << std::endl;
            break;

        case '1':
        case '2':
        case '3':
        case '4':
        case '5':
        case '6': {
            // 切换规划器
            std::vector<std::string> planners = {"Dijkstra", "AStar", "HybridAStar", "ThetaStar", "RRTStar", "JPS"};
            int                      index    = key - '1';
            if (index < planners.size())
            {
                std::string newPlannerName = planners[index];
                std::cout << "Switching to planner: " << newPlannerName << std::endl;

                if (newPlannerName != currentPlannerName)
                {
                    currentPlannerName = newPlannerName;
                    currentPlanner     = createPlanner(currentPlannerName, map);
                    selectedPoints.clear(); // 清除之前的规划结果
                    lastPlanningResult = PlanningResult();
                    currentExplorationNodes.clear();
                    planningInProgress = false;
                    std::cout << "Switched to planner: " << currentPlannerName << std::endl;

                    // 更新显示
                    visualizer->updateDisplay(lastPlanningResult, Point(), Point(), currentPlannerName);
                }
                else
                {
                    std::cout << "Already using planner: " << currentPlannerName << std::endl;
                }
            }
            break;
        }

        default:
            // 忽略其他按键
            break;
        }
    }

    std::cout << "Exiting path planning demo" << std::endl;
    visualizer->cleanup();
    return 0;
}